#include "schedule.h"
#include "list.h"
#include <string.h>
#include <stdbool.h>
#define OS_PRIORITY_MAX 32
#define HIGTH_PROTITY_MASK  (0x80000000)
extern struct osTask g_osTask;
static struct list_node g_task_prority_table[OS_PRIORITY_MAX];
static unsigned int g_task_pri_bitmap = 0;
extern struct taskTCB* g_idleTaskTCB;
extern void tringSvcHandler(void);
void scheduleInit(void)
{
    int pri;
    for(pri = 0; pri < OS_PRIORITY_MAX; pri++){
        ListInit(&g_task_prority_table[pri]);
    }
    g_task_pri_bitmap = 0;
}


static void scheduleInsetHead(struct taskTCB* task)
{
    uint32_t priority = task->priority;
    if(ListEmpty(&g_task_prority_table[priority])){ 
        g_task_pri_bitmap |= (HIGTH_PROTITY_MASK >> priority );
    }
    ListAdd(&g_task_prority_table[priority],&task->schedlist);
}

static void scheduleDelete(struct taskTCB* task)
{
    uint32_t priority = task->priority;
    ListDelete(&task->schedlist);
    if(ListEmpty(&g_task_prority_table[priority]) ){
        g_task_pri_bitmap &= ~(HIGTH_PROTITY_MASK >> priority );
    }
}





void scheduleInsert(struct taskTCB* task)
{
    if(task->taskId != g_idleTaskID){
        
        scheduleInsetHead(task);
    }
    task->state |= TASK_STATUS_READY;

}

void scheduleRemove(struct taskTCB* task)
{
    if(task->taskId != g_idleTaskID){
         scheduleDelete(task);
    }
    task->state &= ~(TASK_STATUS_READY);
}



struct taskTCB* getScheduleTaskTop(void)
{
    uint32_t pri = 0;
    struct taskTCB* taskNew = NULL;
    if(g_task_pri_bitmap != 0){
        pri = __builtin_clz(g_task_pri_bitmap);
        taskNew = LIST_NODE_ENTRY( ((struct list_node* )&g_task_prority_table[pri])->pstNext ,struct taskTCB,schedlist );
    }else{
        taskNew = g_idleTaskTCB;
    }
    return taskNew;
}

extern int osScheduleTaskSw(void);
int osScheduleTaskSw(void)
{
    int32_t status = 0;
    struct taskTCB* taskRun = g_osTask.taskRun;
    if(TASK_STATUS_DELAY & taskRun->state){
        check_timer_tick(taskRun);
    }
    struct taskTCB* taskNew = getScheduleTaskTop();
    g_osTask.taskNew  = taskNew;
    if(taskNew != taskRun){
        taskRun->state &= ~(TASK_STATUS_RUNNING);
        taskNew->state |= TASK_STATUS_RUNNING;
        status = 1;
    }
    scheduleRemove(taskNew);
    return status;
}



void schedule(void)
{     
    tringSvcHandler();
}


